#!/usr/bin/env python

import roslib; 
roslib.load_manifest("EventPlanner")
import rospy
import tf

from geometry_msgs.msg import Quaternion, Point
from interactive_markers.interactive_marker_server import *
from interactive_markers.menu_handler import *
from move_base_msgs.msg import MoveBaseGoal
from EventPlanner.msg import MoveBaseServerGoal
server = None
movePR2 = None

def processFeedback( feedback ):
    if feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
	position = feedback.pose.position
	frame_id = feedback.header.frame_id

	baseGoal = MoveBaseServerGoal()

	goal = MoveBaseGoal()
	goal.target_pose.pose.position = position
	quaternion = tf.transformations.quaternion_about_axis(0.0,(0,0,1))
	goal.target_pose.pose.orientation = Quaternion(*quaternion)
	goal.target_pose.header.frame_id = frame_id
	goal.target_pose.header.stamp = rospy.Time.now()

	baseGoal.goal = goal
	baseGoal.overridePrevious = True
	baseGoal.lookAtGoal = True
	baseGoal.moveAroundGoal = True
	baseGoal.minDistanceFromGoal = 3.0
	baseGoal.updateOrientation = True

	movePR2.publish(baseGoal)
	#movePR2.moveBaseTo(goal,3.0,True,True,True)

def makeBox( msg ):
    marker = Marker()

    marker.type = Marker.CUBE
    marker.scale.x = msg.scale * 0.45
    marker.scale.y = msg.scale * 0.45
    marker.scale.z = msg.scale * 0.45
    marker.color.r = 0.5
    marker.color.g = 0.5
    marker.color.b = 0.5
    marker.color.a = 1.0

    return marker

def makeBoxControl( msg ):
    control =  InteractiveMarkerControl()
    control.always_visible = True
    control.markers.append( makeBox(msg) )
    msg.controls.append( control )
    return control

def make6DofMarker( fixed ):
    int_marker = InteractiveMarker()
    int_marker.header.frame_id = "/map"
    int_marker.pose.position.x = 1.0
    int_marker.pose.position.y = 1.0
    int_marker.scale = 1

    int_marker.name = "simple_6dof"
    int_marker.description = "Simple 6-DOF Control"

    # insert a box
    makeBoxControl(int_marker)

    if fixed:
        int_marker.name += "_fixed"
        int_marker.description += "\n(fixed orientation)"

    control = InteractiveMarkerControl()
    control.orientation.w = 1
    control.orientation.x = 1
    control.orientation.y = 0
    control.orientation.z = 0
    control.name = "rotate_x"
    control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
    if fixed:
        control.orientation_mode = InteractiveMarkerControl.FIXED
    int_marker.controls.append(control)

    control = InteractiveMarkerControl()
    control.orientation.w = 1
    control.orientation.x = 1
    control.orientation.y = 0
    control.orientation.z = 0
    control.name = "move_x"
    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
    if fixed:
        control.orientation_mode = InteractiveMarkerControl.FIXED
    int_marker.controls.append(control)

    control = InteractiveMarkerControl()
    control.orientation.w = 1
    control.orientation.x = 0
    control.orientation.y = 1
    control.orientation.z = 0
    control.name = "rotate_z"
    control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
    if fixed:
        control.orientation_mode = InteractiveMarkerControl.FIXED
    int_marker.controls.append(control)

    control = InteractiveMarkerControl()
    control.orientation.w = 1
    control.orientation.x = 0
    control.orientation.y = 1
    control.orientation.z = 0
    control.name = "move_z"
    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
    if fixed:
        control.orientation_mode = InteractiveMarkerControl.FIXED
    int_marker.controls.append(control)

    control = InteractiveMarkerControl()
    control.orientation.w = 1
    control.orientation.x = 0
    control.orientation.y = 0
    control.orientation.z = 1
    control.name = "rotate_y"
    control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
    if fixed:
        control.orientation_mode = InteractiveMarkerControl.FIXED
    int_marker.controls.append(control)

    control = InteractiveMarkerControl()
    control.orientation.w = 1
    control.orientation.x = 0
    control.orientation.y = 0
    control.orientation.z = 1
    control.name = "move_y"
    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
    if fixed:
        control.orientation_mode = InteractiveMarkerControl.FIXED
    int_marker.controls.append(control)

    server.insert(int_marker, processFeedback)

def makeViewFacingMarker():
    int_marker = InteractiveMarker()
    int_marker.header.frame_id = "/map"
    int_marker.pose.position.x = 1.0
    int_marker.pose.position.y = 1.0
    int_marker.scale = 1

    int_marker.name = "view_facing"
    int_marker.description = "View Facing 6-DOF"

    # make a control that rotates around the view axis
    control = InteractiveMarkerControl()
    control.orientation_mode = InteractiveMarkerControl.VIEW_FACING
    control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
    control.orientation.w = 1
    control.name = "rotate"
    int_marker.controls.append(control)

    # create a box in the center which should not be view facing,
    # but move in the camera plane.
    control = InteractiveMarkerControl()
    control.orientation_mode = InteractiveMarkerControl.VIEW_FACING
    control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
    control.independent_marker_orientation = True
    control.name = "move"
    control.markers.append( makeBox(int_marker) )
    control.always_visible = True
    int_marker.controls.append(control)

    server.insert(int_marker, processFeedback)
if __name__=="__main__":
    rospy.init_node("follow_me")
    movePR2 = rospy.Publisher("moveBase",MoveBaseServerGoal);
    server  = InteractiveMarkerServer("basic_controls")
    #make6DofMarker( True )
    makeViewFacingMarker()
    server.applyChanges()

    rospy.spin()
    
